#ifndef _CALIBRATION__SENSOR_CALI_H_
#define _CALIBRATION__SENSOR_CALI_H_

#include <stdint.h>
#include "arm_math.h"

enum {
	CALI_PROCESSING = 0,
	CALI_FAILED,
	CALI_DONE,
	CALI_REQUIRE
};

struct ellipsoid_param {
	float scale[3];
	float offset[3];
};

uint8_t mag_check_calibration(int16_t *raw,
			      struct ellipsoid_param *res,
			      uint8_t do_cali);
void mag_compensation(int16_t *raw, struct ellipsoid_param *cali);

uint8_t acc_calibration(int16_t *raw,
			struct ellipsoid_param *res,
			uint8_t do_cali);
void acc_compensation(int16_t *raw, struct ellipsoid_param *res);

static inline float vec3_norm(float x, float y, float z)
{
	return (x * x + y * y + z * z);
}

static inline void state_to_param(float *v, struct ellipsoid_param *res)
{
	float gamma = 0.0f;
	res->offset[0] = -v[3]/v[0];
	res->offset[1] = -v[4]/v[1];
	res->offset[2] = -v[5]/v[2];

	gamma = 1 + v[3]*v[3]/v[0] + v[4]*v[4]/v[1] + v[5]*v[5]/v[2];
	arm_sqrt_f32(gamma/v[0], &(res->scale[0]));
	arm_sqrt_f32(gamma/v[1], &(res->scale[1]));
	arm_sqrt_f32(gamma/v[2], &(res->scale[2]));
}

static inline void param_to_state(struct ellipsoid_param *res, float *v)
{
	float yita;
	float a_u, b_v, c_w;
	if (res->scale[0] < 1e-7 && res->scale[0] > -1e-7)
		return;
	if (res->scale[1] < 1e-7 && res->scale[1] > -1e-7)
		return;
	if (res->scale[2] < 1e-7 && res->scale[2] > -1e-7)
		return;

	a_u = res->offset[0] / res->scale[0];
	b_v = res->offset[1] / res->scale[1];
	c_w = res->offset[2] / res->scale[2];
	yita = 1.0f - a_u * a_u - b_v * b_v - c_w * c_w;

	if (yita < 1e-7 && yita > -1e-7)
		return;

	v[0] = 1.0f/(res->scale[0] * res->scale[0] * yita);
	v[1] = 1.0f/(res->scale[1] * res->scale[1] * yita);
	v[2] = 1.0f/(res->scale[2] * res->scale[2] * yita);
	v[3] = -res->offset[0] * v[0];
	v[4] = -res->offset[1] * v[1];
	v[5] = -res->offset[2] * v[2];
}

#endif
